#include "control.h"
#include "filter.h"
#include "mpu6050.h"
#include "math.h"
//#include "outputdata.h"
#include "tim.h"
#include "main.h"

short x_nAcc,y_nAcc,z_nAcc;
short x_nGyro,y_nGyro,z_nGyro;
float x_fAcc,y_fAcc,z_fAcc;

float g_fAccAngle;
float g_fGyroAngleSpeed;
float g_fCarAngle;
float dt = 0.005;


unsigned int g_nLeftMotorPulse ;  //save the pulse of left motor

int g_nSpeedTarget = 0;          //the target speed of left motor
int g_nLeftMotorOutput;

int nErrorPrev;                  //Error of past
int nPwmBais, nPwm;              //the increment of PWM

unsigned char g_ucMainEventCount; 

void GetMotorPulse(void)
{
		g_nLeftMotorPulse = (short)(__HAL_TIM_GET_COUNTER(&htim4));     //Get pulse of motor
		__HAL_TIM_SET_COUNTER(&htim4, 0);                               //To make sure the motor pulse be setted in 0
}

int SpeedInnerControl(int nPulse, int nTarget)
{
	  
		int nError;
		float fP = 45, fI = 3.0;
		
		nError = nPulse - nTarget;
      
	  nPwmBais = fP * (nError - nErrorPrev) + fI * nError;           // the equation of increment PI
	  
	  nErrorPrev = nError;                                           //save previous error  
	  
	  nPwm += nPwmBais;
	
	  if(nPwm > 999)  nPwm = 999;
	  if(nPwm < -999) nPwm = 999;
 
		return nPwm;
}

void SetMotorVoltAndDre(int nMotorPwm)
{
    if(nMotorPwm < 0)
		{
			HAL_GPIO_WritePin(BIN1_GPIO_Port, BIN1_Pin, GPIO_PIN_SET);
			HAL_GPIO_WritePin(BIN2_GPIO_Port, BIN2_Pin, GPIO_PIN_RESET);
			nMotorPwm = (-nMotorPwm);
			__HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_2, nMotorPwm);
		}else
		{
			HAL_GPIO_WritePin(BIN1_GPIO_Port, BIN1_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(BIN2_GPIO_Port, BIN2_Pin, GPIO_PIN_SET);
			__HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_2, nMotorPwm);
		}
}


void GetMpuData(void)//读取MPU-6050数据
{
			MPU_Get_Accelerometer(&x_nAcc, &y_nAcc, &z_nAcc);//获取MPU6050加速度数据
			MPU_Get_Gyroscope(&x_nGyro, &y_nGyro, &z_nGyro); //获取MPU6050陀螺仪数据
}

void AngleCalculate(void)//角度计算函数
{
    //-------加速度数据处理--------------------------
    //量程为±2g时，灵敏度：16384 LSB/g
			x_fAcc = x_nAcc / 16384.0;
			y_fAcc = y_nAcc / 16384.0;
			z_fAcc = z_nAcc / 16384.0;

			g_fAccAngle = atan2(y_fAcc,z_fAcc) * 180.0 / 3.14;

			//-------陀螺仪数据处理-------------------------
			//范围为2000deg/s时，换算关系：16.4 LSB/(deg/s)
			g_fGyroAngleSpeed = x_nGyro / 16.4;//计算角速度值                    

			//-------互补滤波---------------
			g_fCarAngle = ComplementaryFilter(g_fAccAngle, g_fGyroAngleSpeed, dt);
}


